function findPlane_RANSAC

tic     %start timer
clear   %clear workspace

%---parameterization---%
%n = 11;        %use for create random 3D point      
k1 = 0;
k2 = 0;
j=0;
stop_round = 7; % chosen from table in text : 20% of datas are outlier and 3 sample points 
threshold = 1;  % threshold use to determine the inlier
numInlier = 0;  % number of inlier
min_inlier = 9;
state = 0;

%---create random 3D points---%
%test point along X,Y,Z direction
%--------for XY plane
all_point = [1 2 3 4 5 6 18 19 50 38 17; 9 5 19 20 60 3 56 48 38 93 12; 0 0 0 0 0 0 0 -5 6 0 0];

%--------for XZ plane
%all_point = [1 2 3 4 5 6 18 19 50 38 17; 0 0 0 0 0 0 0 -50 50 0 0; 9 5 19 20 60 3 56 48 38 93 12]

%--------for YZ plane
%all_point = [0 0 0 0 0 0 0 -5 6 0 0 ; 1 2 3 4 5 6 18 19 50 38 17; 9 5 19 20 60 3 56 48 38 93 12]

%-----plot input points
figure(1);
plot3(all_point(1,:),all_point(2,:),all_point(3,:),'or');
axis([-100 100 -100 100 -100 100]);
grid on;


%---RANSAC algorithm---%
for m = 1:stop_round                % loop until the loop have to stop
    %------choose 3 random points-------%
    for i = 1:3
        k2 = k1;
        k1 = j;
        j = randint(1,1,[1,size(all_point,2)]);     %random 1 number in range [1,n]
        while (k1==j)||(j==k2)                      % if there is some the same number then random again
            j = randint(1,1,[1,size(all_point,2)]);
        end
        three_points(:,i) = all_point(:,j)      %construct the matrix of random three points from the set of 3D point
    end
    %------------------------------------%
    
    %---calculate the plane from 3 points---%
    v1 = three_points(:,1)-three_points(:,3);       %create the first vector from point 1 and 3
    v2 = three_points(:,2)-three_points(:,3);       %create the second vector from point 2 and 3
    ABCcoff = cross(v1,v2);                         %find the A,B,C coefficient of the plane equation
    Dcoff = dot(-three_points(:,3)',cross(three_points(:,1),three_points(:,2)));    %find the D coefficient of the plane equation
    PlaneCoff = [ABCcoff ; Dcoff]   %combine all of the coefficient of the plane equation
    %---------------------------------------%
    
    %---count inlier---%
    for l = 1:size(all_point,2)
        distance = (PlaneCoff'*[all_point(:,l) ; 1])/(sqrt(sum(ABCcoff.^2)))    % find the distance from the point to the plane
        if (abs(distance) < threshold)
            %Count the number of inlier points for the specific plane
            %If a point is less than the threshold, then it becomes
            %an inlier point
            numInlier = numInlier + 1;      %count the number of inlier 
            inlier_point(:,numInlier) = all_point(:,l);
        end
        %num_inli = numInlier
    end
    %------------------%
    
    %---break if the number of inlier > the minimum number of inlier
    %If it breaks, then a plane has been found
    if numInlier >= min_inlier
        state = 1
        break;
    end
    %------------------------------------------------------------%
    
    %---keep parameter in structure---%
    StrPlane(m).PlaneEqua = PlaneCoff;
    StrPlane(m).inlier = numInlier;
    StrPlane(m).inlierPoint = inlier_point;
    %---------------------------------%
    
    %---reset  parameters---%
    j = 0; k1 = 0; k2 = 0;
    clear three_points;
    clear numInlier;
    numInlier = 0;
    %-----------------------%
end

%If a plane has not been found with the maximum number of inlier points
if state == 0
    %---Find the max inlier Plane equation---%
    [row , col] = size(StrPlane);   %find size of structure strPlane
    for i = 1:col
        inlier(i) = StrPlane(i).inlier;      %list the number of inlier of each plane equation
    end
    [val , pos] = max(inlier) ;      %find position of plane equation that has maximum inlier
    %FinalPlane = StrPlane(pos).PlaneEqua    %return the plane equation that has maximum inlier to the variable finalplane
    Point = [StrPlane(pos).inlierPoint ; ones(1,val)]
    figure(3);
    plot3(Point(1,:),Point(2,:),Point(3,:),'b+');
    axis([-100 100 -100 100 -100 100]);
    grid on;
    FinalPlane = cal_Plane_DLT(Point)
else
    s = size(inlier_point,2);
    Point = [inlier_point ; ones(1,s)]
    figure(3);
    plot3(inlier_point(1,:),inlier_point(2,:),inlier_point(3,:),'b+');
    axis([-100 100 -100 100 -100 100]);
    grid on;
    FinalPlane = cal_Plane_DLT(Point)
end
toc

%plot surface of plane
%------for YZ plane
% y = [-10:10];
% z = [-10,10];
% [Y,Z] = meshgrid(y,z);
% X = (-(FinalPlane(2,1)*Y)-(FinalPlane(3,1)*Z)-FinalPlane(4,1))/FinalPlane(1,1);
%------for XZ plane
% x = [-10;10];
% z = [-10,10];
% [X,Z] = meshgrid(x,z);
% Y = (-(FinalPlane(1,1)*X)-(FinalPlane(3,1)*Z)-FinalPlane(4,1))/FinalPlane(2,1);
%------for XY plane
x = [-10;10];
y = [-10:10];
[X,Y] = meshgrid(x,y);
Z = (-(FinalPlane(1,1)*X)-(FinalPlane(2,1)*Y)-FinalPlane(4,1))/FinalPlane(3,1);

%---plot surface
figure(2);
surf(X,Y,Z)
axis([-10 10 -10 10 -10 10])

%--------END-----------------%
function plane = cal_Plane_DLT(x)
plane = DLT(x');

function plane = DLT(A)
[U,D,V] = svd( A );
plane = V(:,4);

